#!/usr/bin/python3
# rosrun默认是将文件当做shell脚本来运行的，需要我们在头部指定脚本的解释器为/usr/bin/python3
import rospy
from ocs2_msgs.msg import mpc_target_trajectories, mpc_observation, mpc_state, mpc_input
from ocs2_msgs.srv import reset, resetRequest
from threading import Thread, Condition, Lock
import numpy as np

pos = 0
theta1 = 0
theta2 = 0
theta3 = 0
time = 0
# 初始化observer
observer = mpc_observation()
observer.time = 0
observer.state = mpc_state([0, 0, 0, 0, 0, 0, 0, 0])
observer.input = mpc_input([0])

# 线程锁
# 条件锁，可以进行线程间通信
data_con = Condition()
observer_lock = Lock()


def observationCallback(msg):
    global observer
    # 要访问 observer 所以先获得锁，之后再访问 observer
    observer_lock.acquire()
    observer = msg
    # 用完之后要释放锁
    observer_lock.release()


def getInputThread():
    global pos, theta1, theta2, theta3, time
    while True:
        input__str = input("请输入机器人的运动指令(theta1 theta2 theta3 pos time):")
        input__list = input__str.split(" ")
        if not len(input__list) == 5:
            print("输入指令有误，请重新输入")
            continue
        try:
            # 先获得锁
            data_con.acquire()
            theta1 = float(input__list[0])
            theta2 = float(input__list[1])
            theta3 = float(input__list[2])
            pos = float(input__list[3])
            time = float(input__list[4])
            if time == 0:
                raise ValueError("ValueError:time == 0")
            # 使用完成通知出去 然后释放
            data_con.notify()
            data_con.release()
        except ValueError as msg:
            print(msg)
            print("输入指令有误，请重新输入")
            continue


def handleInputThread(publisher: rospy.Publisher):
    global pos, theta1, theta2, theta3, time
    while True:
        # 获得锁
        data_con.acquire()
        # 等待 消息 通知
        data_con.wait()
        observer_lock.acquire()
        # 处理pos和vel
        msg = mpc_target_trajectories()
        delta_time = time
        msg.timeTrajectory = [
            observer.time, observer.time + delta_time,
            observer.time + delta_time + 0.1
        ]
        state = np.array(observer.state.value)
        # target_state__list = np.array(state[:])
        target_state__list = np.array(
            [state[0] + pos, theta1, theta2, theta3, 0, 0, 0, 0])
        target_state_final__list = target_state__list.tolist()
        target_state__list[4:] = (target_state__list[:4] -
                                  state[:4]) / delta_time

        target_state = mpc_state(target_state__list.tolist())
        target_state_final = mpc_state(target_state_final__list)
        target_input = mpc_input([0])
        msg.stateTrajectory = [
            observer.state, target_state, target_state_final
        ]
        msg.inputTrajectory = [observer.input, target_input, target_input]
        publisher.publish(msg)

        # reset_msg = resetRequest()
        # reset_msg.reset = True
        # reset_msg.targetTrajectories = msg
        # reset_srv(reset_msg)
        # 释放锁
        observer_lock.release()
        data_con.release()


if __name__ == "__main__":
    # 初始化节点
    rospy.init_node("get_target")
    rate = rospy.Rate(100)
    # 构建publisher
    reset_srv = rospy.ServiceProxy('/cartpole_mpc_reset', reset)

    pub = rospy.Publisher("/cartpole_target",
                          mpc_target_trajectories,
                          queue_size=10)
    subscribe = rospy.Subscriber("/cartpole_mpc_observation", mpc_observation,
                                 observationCallback)
    # 编写初始化程序
    # 创建线程 其中 daemon 参数 True，主程序退出，子程序也会退出，如果为False 主程序退出 子程序不会退出
    # 这里希望 主程序退出 子程序也退出
    input_thread = Thread(target=getInputThread,
                          daemon=True,
                          name="get input",
                          args=())
    handle_thread = Thread(target=handleInputThread,
                           daemon=True,
                           name="handle thread",
                           args=(pub, ))
    input_thread.start()
    handle_thread.start()
    # 循环
    while not rospy.is_shutdown():
        try:
            rate.sleep()
        except rospy.exceptions.ROSInterruptException:
            print("程序退出")
            break

    # 编写结束程序